import cv2
import numpy as np
class Calibration(object):
    def __init__(self):
        super(Calibration, self).__init__()
        self.b = 0.025
        self.w = 4
        self.h = 5
        self.circleCenters = []
        self.objectPoints = []
        self.objectPointsVec = []
        self.circleCenters_nums = 0
    def findCirclesGrid(self, color_image):
        params = cv2.SimpleBlobDetector_Params()
        params.maxArea = 30000
        params.minArea = 1000
        blobDetector = cv2.SimpleBlobDetector_create(params)
        ret, corners = cv2.findCirclesGrid(image=color_image, patternSize=(self.w, self.h),
                                           flags=cv2.CALIB_CB_ASYMMETRIC_GRID,
                                           blobDetector=blobDetector)
        return ret, corners

    def show(self, color_image, ret, corners):
        self.circleCenters.append(corners)
        self.circleCenters_nums = len(self.circleCenters)
        cv2.drawChessboardCorners(color_image, (self.w, self.h), corners, ret)
        cv2.imshow('camera image', 255 - color_image)
        cv2.waitKey(1000)



    def calibrateCamera(self, calibrationInfoUrl):
        objectPoints = []
        for i in range(self.h):
            for j in range(self.w):
                objectPoints.append([(i % 2 + j * 2) * self.b, i * self.b, 0])
                self.objectPoints = np.float32(objectPoints)
        print(self.objectPoints)
        for i in range(0, len(self.circleCenters)):
            self.objectPointsVec.append(self.objectPoints)
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.objectPointsVec, self.circleCenters,
                                                           (1280, 1024), None, None)
        print('mtx',mtx)
        print('dist',dist)
        print('ret',ret)
        if ret < 1:
            print(calibrationInfoUrl)
            fs = cv2.FileStorage(calibrationInfoUrl, cv2.FileStorage_WRITE)
            fs.write('cameraMatrix', mtx)
            fs.write('distCoeffs', dist)
            result = fs
            return result
